import sensor, image, time
from machine import UART
import display
from pyb import UART
import math

# 颜色阈值
greenblack_threshold = (2, 100, -128, -25, -128, 127) #绿激光
redblack_threshold =(2, 100, 19, 127, -128, 127)#红激光

# 初始化摄像头
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(200)
sensor.set_auto_whitebal(False)  # 禁用白平衡
sensor.set_auto_gain(False)  # 关闭自动增益
sensor.set_auto_exposure(False, exposure_us=2000)  # 设置曝光度
sensor.set_windowing(50, 0, 240, 210)

clock = time.clock()
# 初始化串口
uart = UART(3, 115200, timeout_char=1000)

# 初始化显示屏
lcd = display.SPIDisplay()

d = {}
mode = 0
last_mode=0
rx_buff = []
state = 0
res = 0
start = 0

red_and_green_point=[0]*4
# 串口接收函数
def Receive_Prepare(da):
    global state, mode, start
    if state == 0 and da == 0x0d:  # 帧头
        state = 1
    elif state == 1:
        rx_buff.append(da)
        state = 2
    elif state == 2 and da == 0x0e:
        state = 0
        if rx_buff[0] == 0x23:
            pass
            # mode=1
            # start=0
        rx_buff.clear()
    else:
        state = 0
        rx_buff.clear()

while True:
    img = sensor.snapshot()

    #找红点激光
    redblack_blobs = img.find_blobs([redblack_threshold], merge=20)
    if redblack_blobs:
        for a in redblack_blobs:
            red_and_green_point[0]=a[5]
            red_and_green_point[1]=a[6]
            print(red_and_green_point[0], red_and_green_point[1])
            img.draw_cross(red_and_green_point[0], red_and_green_point[1], color=(0, 0, 255))

    greenblack_blobs = img.find_blobs([greenblack_threshold], merge=20)
    if greenblack_blobs:
        for a in greenblack_blobs:
            red_and_green_point[2]=a[5]
            red_and_green_point[3]=a[6]
            print(red_and_green_point[2], red_and_green_point[3])
            img.draw_cross(red_and_green_point[2], red_and_green_point[3], color=(0, 255, 0))


    if(greenblack_blobs and redblack_blobs):
        data = bytearray([0xA4, 0xA4, red_and_green_point[0],red_and_green_point[1], red_and_green_point[2],red_and_green_point[3],0x0D, 0x0B])
        uart.write(data)


    # 显示图像
    lcd.write(img)
    img.rotation_corr(z_rotation=180)  # 翻转图像



